#include "CenterOfMassSensorVisualisationFactory.h"
#include "CenterOfMassSensorVisualisation.h"

#include <boost/extension/extension.hpp>

#include <MMM/Motion/Plugin/WholeBodyDynamicPlugin/WholeBodyDynamicSensor.h>

using namespace MMM;

// register this factory
SensorVisualisationFactory::SubClassRegistry CenterOfMassSensorVisualisationFactory::registry(VIS_STR(WholeBodyDynamicSensor::TYPE), &CenterOfMassSensorVisualisationFactory::createInstance);

CenterOfMassSensorVisualisationFactory::CenterOfMassSensorVisualisationFactory() : SensorVisualisationFactory() {}

CenterOfMassSensorVisualisationFactory::~CenterOfMassSensorVisualisationFactory() = default;

std::string CenterOfMassSensorVisualisationFactory::getName()
{
    return VIS_STR(WholeBodyDynamicSensor::TYPE);
}

SensorVisualisationPtr CenterOfMassSensorVisualisationFactory::createSensorVisualisation(SensorPtr sensor, VirtualRobot::RobotPtr robot, VirtualRobot::CoinVisualizationPtr visualization, SoSeparator* sceneSep) {
    WholeBodyDynamicSensorPtr s = boost::dynamic_pointer_cast<WholeBodyDynamicSensor>(sensor);
    if (!s) {
        MMM_ERROR << sensor->getType() << " could not be castet to " << WholeBodyDynamicSensor::TYPE << std::endl;
        return nullptr;
    }
    return SensorVisualisationPtr(new CenterOfMassSensorVisualisation(s, sceneSep));
}

SensorVisualisationFactoryPtr CenterOfMassSensorVisualisationFactory::createInstance(void *)
{
    return SensorVisualisationFactoryPtr(new CenterOfMassSensorVisualisationFactory());
}

extern "C"
BOOST_EXTENSION_EXPORT_DECL SensorVisualisationFactoryPtr getFactory() {
    return SensorVisualisationFactoryPtr(new CenterOfMassSensorVisualisationFactory());
}

extern "C"
BOOST_EXTENSION_EXPORT_DECL std::string getVersion() {
    return SensorVisualisationFactory::VERSION;
}
